%%
clear;clc

Xc    = 0;
Yc    = 0;
Zc    = 100;

roll  = 10 * pi/180;
pitch = 180 * pi/180;
yaw   = 0 * pi/180;

f  = 4000;
cx = 2000;
cy = 1500;

pixx = 2000;
pixy = 1500;

cx = cx/f;
cy = cy/f;
pixx = pixx/f;
pixy = pixy/f;
f = 1;

Xw = -(Zc*pixx*cos(pitch)*cos(roll)^2*cos(yaw) - Xc*f*cos(pitch)*cos(roll)*cos(yaw)^2 - Zc*cx*cos(pitch)*cos(roll)^2*cos(yaw) + Xc*cy*cos(pitch)*cos(yaw)^2*sin(roll) - Zc*cx*cos(pitch)*cos(yaw)*sin(roll)^2 + Zc*cy*cos(pitch)^2*cos(roll)*sin(yaw) - Xc*f*cos(pitch)*cos(roll)*sin(yaw)^2 - Xc*pixy*cos(pitch)*cos(yaw)^2*sin(roll) + Zc*pixx*cos(pitch)*cos(yaw)*sin(roll)^2 - Zc*pixy*cos(pitch)^2*cos(roll)*sin(yaw) + Xc*cy*cos(pitch)*sin(roll)*sin(yaw)^2 + Zc*cy*cos(roll)*sin(pitch)^2*sin(yaw) + Zc*f*cos(pitch)^2*sin(roll)*sin(yaw) - Xc*pixy*cos(pitch)*sin(roll)*sin(yaw)^2 - Zc*pixy*cos(roll)*sin(pitch)^2*sin(yaw) + Zc*f*sin(pitch)^2*sin(roll)*sin(yaw) - Xc*cx*cos(roll)^2*cos(yaw)^2*sin(pitch) + Xc*pixx*cos(roll)^2*cos(yaw)^2*sin(pitch) - Xc*cx*cos(roll)^2*sin(pitch)*sin(yaw)^2 - Xc*cx*cos(yaw)^2*sin(pitch)*sin(roll)^2 + Xc*pixx*cos(roll)^2*sin(pitch)*sin(yaw)^2 + Xc*pixx*cos(yaw)^2*sin(pitch)*sin(roll)^2 - Xc*cx*sin(pitch)*sin(roll)^2*sin(yaw)^2 + Xc*pixx*sin(pitch)*sin(roll)^2*sin(yaw)^2 + Zc*f*cos(roll)*cos(yaw)*sin(pitch) - Zc*cy*cos(yaw)*sin(pitch)*sin(roll) + Zc*pixy*cos(yaw)*sin(pitch)*sin(roll))/(cx*cos(roll)^2*cos(yaw)^2*sin(pitch) - pixx*cos(roll)^2*cos(yaw)^2*sin(pitch) + cx*cos(roll)^2*sin(pitch)*sin(yaw)^2 + cx*cos(yaw)^2*sin(pitch)*sin(roll)^2 - pixx*cos(roll)^2*sin(pitch)*sin(yaw)^2 - pixx*cos(yaw)^2*sin(pitch)*sin(roll)^2 + cx*sin(pitch)*sin(roll)^2*sin(yaw)^2 - pixx*sin(pitch)*sin(roll)^2*sin(yaw)^2 + f*cos(pitch)*cos(roll)*cos(yaw)^2 - cy*cos(pitch)*cos(yaw)^2*sin(roll) + f*cos(pitch)*cos(roll)*sin(yaw)^2 + pixy*cos(pitch)*cos(yaw)^2*sin(roll) - cy*cos(pitch)*sin(roll)*sin(yaw)^2 + pixy*cos(pitch)*sin(roll)*sin(yaw)^2);
Yw = (Zc*cy*cos(pitch)^2*cos(roll)*cos(yaw) + Yc*f*cos(pitch)*cos(roll)*cos(yaw)^2 - Zc*pixy*cos(pitch)^2*cos(roll)*cos(yaw) - Yc*cy*cos(pitch)*cos(yaw)^2*sin(roll) + Zc*cx*cos(pitch)*cos(roll)^2*sin(yaw) + Zc*cy*cos(roll)*cos(yaw)*sin(pitch)^2 + Yc*f*cos(pitch)*cos(roll)*sin(yaw)^2 + Zc*f*cos(pitch)^2*cos(yaw)*sin(roll) + Yc*pixy*cos(pitch)*cos(yaw)^2*sin(roll) - Zc*pixx*cos(pitch)*cos(roll)^2*sin(yaw) - Zc*pixy*cos(roll)*cos(yaw)*sin(pitch)^2 - Yc*cy*cos(pitch)*sin(roll)*sin(yaw)^2 + Zc*cx*cos(pitch)*sin(roll)^2*sin(yaw) + Zc*f*cos(yaw)*sin(pitch)^2*sin(roll) + Yc*pixy*cos(pitch)*sin(roll)*sin(yaw)^2 - Zc*pixx*cos(pitch)*sin(roll)^2*sin(yaw) + Yc*cx*cos(roll)^2*cos(yaw)^2*sin(pitch) - Yc*pixx*cos(roll)^2*cos(yaw)^2*sin(pitch) + Yc*cx*cos(roll)^2*sin(pitch)*sin(yaw)^2 + Yc*cx*cos(yaw)^2*sin(pitch)*sin(roll)^2 - Yc*pixx*cos(roll)^2*sin(pitch)*sin(yaw)^2 - Yc*pixx*cos(yaw)^2*sin(pitch)*sin(roll)^2 + Yc*cx*sin(pitch)*sin(roll)^2*sin(yaw)^2 - Yc*pixx*sin(pitch)*sin(roll)^2*sin(yaw)^2 - Zc*f*cos(roll)*sin(pitch)*sin(yaw) + Zc*cy*sin(pitch)*sin(roll)*sin(yaw) - Zc*pixy*sin(pitch)*sin(roll)*sin(yaw))/(cx*cos(roll)^2*cos(yaw)^2*sin(pitch) - pixx*cos(roll)^2*cos(yaw)^2*sin(pitch) + cx*cos(roll)^2*sin(pitch)*sin(yaw)^2 + cx*cos(yaw)^2*sin(pitch)*sin(roll)^2 - pixx*cos(roll)^2*sin(pitch)*sin(yaw)^2 - pixx*cos(yaw)^2*sin(pitch)*sin(roll)^2 + cx*sin(pitch)*sin(roll)^2*sin(yaw)^2 - pixx*sin(pitch)*sin(roll)^2*sin(yaw)^2 + f*cos(pitch)*cos(roll)*cos(yaw)^2 - cy*cos(pitch)*cos(yaw)^2*sin(roll) + f*cos(pitch)*cos(roll)*sin(yaw)^2 + pixy*cos(pitch)*cos(yaw)^2*sin(roll) - cy*cos(pitch)*sin(roll)*sin(yaw)^2 + pixy*cos(pitch)*sin(roll)*sin(yaw)^2);

fprintf('Xw = %.3f\nYw = %.3f\n',Xw,Yw);

Math.PI = pi;
Math.cos = @cos;
Math.sin = @sin;

this.IO.cx = cx;
this.IO.cy = cy;
this.IO.f  = f;

this.EO.Xc    = Xc;
this.EO.Yc    = Yc;
this.EO.Zc    = Zc;
this.EO.roll  = roll;
this.EO.pitch = pitch;
this.EO.yaw   = yaw;

Xw = -(this.EO.Zc*pixx*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw) - this.EO.Xc*this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw)^2 - this.EO.Zc*this.IO.cx*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw) + this.EO.Xc*this.IO.cy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) - this.EO.Zc*this.IO.cx*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)*Math.sin(this.EO.roll)^2 + this.EO.Zc*this.IO.cy*Math.cos(this.EO.pitch)^2*Math.cos(this.EO.roll)*Math.sin(this.EO.yaw) - this.EO.Xc*this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.sin(this.EO.yaw)^2 - this.EO.Xc*pixy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) + this.EO.Zc*pixx*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)*Math.sin(this.EO.roll)^2 - this.EO.Zc*pixy*Math.cos(this.EO.pitch)^2*Math.cos(this.EO.roll)*Math.sin(this.EO.yaw) + this.EO.Xc*this.IO.cy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2 + this.EO.Zc*this.IO.cy*Math.cos(this.EO.roll)*Math.sin(this.EO.pitch)^2*Math.sin(this.EO.yaw) + this.EO.Zc*this.IO.f*Math.cos(this.EO.pitch)^2*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw) - this.EO.Xc*pixy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2 - this.EO.Zc*pixy*Math.cos(this.EO.roll)*Math.sin(this.EO.pitch)^2*Math.sin(this.EO.yaw) + this.EO.Zc*this.IO.f*Math.sin(this.EO.pitch)^2*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw) - this.EO.Xc*this.IO.cx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) + this.EO.Xc*pixx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) - this.EO.Xc*this.IO.cx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 - this.EO.Xc*this.IO.cx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 + this.EO.Xc*pixx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 + this.EO.Xc*pixx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 - this.EO.Xc*this.IO.cx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 + this.EO.Xc*pixx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 + this.EO.Zc*this.IO.f*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw)*Math.sin(this.EO.pitch) - this.EO.Zc*this.IO.cy*Math.cos(this.EO.yaw)*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll) + this.EO.Zc*pixy*Math.cos(this.EO.yaw)*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll))/(this.IO.cx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) - pixx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) + this.IO.cx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 + this.IO.cx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 - pixx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 - pixx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 + this.IO.cx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 - pixx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 + this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw)^2 - this.IO.cy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) + this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.sin(this.EO.yaw)^2 + pixy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) - this.IO.cy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2 + pixy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2);

Yw = (this.EO.Zc*this.IO.cy*Math.cos(this.EO.pitch)^2*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw) + this.EO.Yc*this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw)^2 - this.EO.Zc*pixy*Math.cos(this.EO.pitch)^2*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw) - this.EO.Yc*this.IO.cy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) + this.EO.Zc*this.IO.cx*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)^2*Math.sin(this.EO.yaw) + this.EO.Zc*this.IO.cy*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw)*Math.sin(this.EO.pitch)^2 + this.EO.Yc*this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.sin(this.EO.yaw)^2 + this.EO.Zc*this.IO.f*Math.cos(this.EO.pitch)^2*Math.cos(this.EO.yaw)*Math.sin(this.EO.roll) + this.EO.Yc*pixy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) - this.EO.Zc*pixx*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)^2*Math.sin(this.EO.yaw) - this.EO.Zc*pixy*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw)*Math.sin(this.EO.pitch)^2 - this.EO.Yc*this.IO.cy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2 + this.EO.Zc*this.IO.cx*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw) + this.EO.Zc*this.IO.f*Math.cos(this.EO.yaw)*Math.sin(this.EO.pitch)^2*Math.sin(this.EO.roll) + this.EO.Yc*pixy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2 - this.EO.Zc*pixx*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw) + this.EO.Yc*this.IO.cx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) - this.EO.Yc*pixx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) + this.EO.Yc*this.IO.cx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 + this.EO.Yc*this.IO.cx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 - this.EO.Yc*pixx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 - this.EO.Yc*pixx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 + this.EO.Yc*this.IO.cx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 - this.EO.Yc*pixx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 - this.EO.Zc*this.IO.f*Math.cos(this.EO.roll)*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw) + this.EO.Zc*this.IO.cy*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw) - this.EO.Zc*pixy*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw))/(this.IO.cx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) - pixx*Math.cos(this.EO.roll)^2*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch) + this.IO.cx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 + this.IO.cx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 - pixx*Math.cos(this.EO.roll)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.yaw)^2 - pixx*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2 + this.IO.cx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 - pixx*Math.sin(this.EO.pitch)*Math.sin(this.EO.roll)^2*Math.sin(this.EO.yaw)^2 + this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.cos(this.EO.yaw)^2 - this.IO.cy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) + this.IO.f*Math.cos(this.EO.pitch)*Math.cos(this.EO.roll)*Math.sin(this.EO.yaw)^2 + pixy*Math.cos(this.EO.pitch)*Math.cos(this.EO.yaw)^2*Math.sin(this.EO.roll) - this.IO.cy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2 + pixy*Math.cos(this.EO.pitch)*Math.sin(this.EO.roll)*Math.sin(this.EO.yaw)^2);
